#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>		 
#include <fcntl.h>
#include <pthread.h>
#include <termios.h>
#include <errno.h>
#include "serial-ops.h"

#define BUFFER_LENGTH  2048
#define BUFFER_SIZE 64						// 存储单条数据中rtcm头地址的缓存数组

char data_buffer[BUFFER_LENGTH];			// 缓存串口传来的数据，用来校验、拼装、解析gga和rtcm
int data_buffer_rear_index = 0;             // 指向当前缓存的数据结束后的第一个字节

char imu_data[BUFFER_LENGTH];
char gga_data[BUFFER_LENGTH];
char rtcm_datas[BUFFER_SIZE][BUFFER_LENGTH];
int calculated_rtcm_total_lengths[BUFFER_SIZE] = {0}; //抽取得到的rtcm的长度，包括头，体和校验码

int current_rtcm_no = 0;
int current_calculated_rtcm_total_length = 0;


const char GGA_HEAD[] = 
{
    0x24,
    0x47,
    0x4E,
    0x47,
    0x47,
    0x41,
};

const char IMU_HEAD[] = 
{
    0x24,
    0x47,
    0x50,
    0x49,
    0x4D,
    0x55,
};

const char RTCM_HEAD[] = 
{
    0xD3,
    0x00
};

/*
 * 来的数据，先追加到缓存中
 */
void extract_data(char *data, int data_length)
{
    append_to_buffer(data, data_length);

    try_extract_data(); //迭代抽取imu、gga和rtcm数据
}

/*
 * 将来的数据追加到buffer中，然后进行索引
 */
void append_to_buffer(char *data, int data_length)
{ 
    int i;
    for (i = 0; i < data_length; i++)
    {
        data_buffer[data_buffer_rear_index++] = *(data + i);
        if (data_buffer_rear_index == BUFFER_LENGTH)
        {
            return;
        }
    }

	printf("data_buffer = %s\n", data_buffer);
}

/* 
迭代抽取IMU，GGA和RTCM数据
 */
void try_extract_data()
{
	int first_0d0a = -1;

    format_buffer_data(); //格式化buffer数组

    if (data_buffer_rear_index == 0)
    { //格式化后无有效数据，直接返回
        return;
    }

    first_0d0a = indexof_first_0d0a(data_buffer, data_buffer_rear_index);

    if (startwith_imu_head(data_buffer))
    {
        if (first_0d0a == -1)
        { //没有检索到回车换行符，直接返回
            return;
        }
        //extract_imu(callback);
    }

    if (startwith_gga_head(data_buffer))
    {
		printf("startwith_gga_head\n");

        if (first_0d0a == -1)
        { //没有检索到回车换行符，直接返回
            return;
        }
        extract_gga();
    }

    if (startwith_rtcm_head(data_buffer))
    {
        printf("startwith_rtcm_head\n");
        extract_rtcm();

        if (data_buffer_rear_index < 4)
        {
            return;
        }

        if (current_calculated_rtcm_total_length > data_buffer_rear_index)
        {
            return;
        }
    }

    if (data_buffer_rear_index > 0)
    {
        try_extract_data();
    }
}

void extract_gga()
{
    //抽取gga;
    int gga_length = indexof_first_0d0a(data_buffer, data_buffer_rear_index);
    if (gga_length > BUFFER_LENGTH)
    {
        gga_length = BUFFER_LENGTH;
    }
    if (gga_length == -1)
    {
        return;
    }
    int i;
    for (i = 0; i < gga_length; i++)
    {
        gga_data[i] = data_buffer[i];
    }
    gga_data[i] = '\0';

	//printf("finish gga === %s\n", gga_data);

    //if (callback.callback_ptr_gga != NULL)
    {
        //callback.callback_ptr_gga(gga_data);
    }

    current_rtcm_no = 0;
	data_buffer_rear_index-=gga_length;
    data_buffer[0] = '\0';
    format_buffer_data();
    // printf("%d \n", data_buffer_rear_index);
}

/* 
抽取rtcm数据
 */
void extract_rtcm()
{
	int rtcm_length = 0;
	int rtcm_total_length = 0;
	int i = 0;

    if (data_buffer_rear_index < 4)
    {
        return;
    }

    
    rtcm_length += (0x03 & (*(data_buffer + 1))) << 4;
    rtcm_length += (0xff & *(data_buffer + 2));

    rtcm_total_length = get_rtcm_total_length(rtcm_length);
    current_calculated_rtcm_total_length = rtcm_total_length;

    if (data_buffer_rear_index < rtcm_total_length)
    {
        return;
    }

    char *checksum = get_checksum(data_buffer, rtcm_total_length);
    char checksum_01 = *(data_buffer + rtcm_total_length - 3);
    char checksum_02 = *(data_buffer + rtcm_total_length - 2);
    char checksum_03 = *(data_buffer + rtcm_total_length - 1);

    printf("%02x \n", 0xff & checksum_01);
    printf("%02x \n", 0xff & checksum_02);
    printf("%02x \n", 0xff & checksum_03);

    if ((checksum[0] == checksum_01) && (checksum[1] == checksum_02) && (checksum[2] == checksum_03))
    {
        // ALOGE("RTCM:data_buffer_rear_index %d,rtcm_total_length %d", data_buffer_rear_index, rtcm_total_length);
        // ALOGE("RTCMALL:%s", BinToHexString(data_buffer, data_buffer_rear_index));
        int rtcm_type = getRtcmType(data_buffer);
        if (rtcm_type == 1074 || rtcm_type == 1075)
        {
            current_rtcm_no = 0;
        }

        else if (rtcm_type == 1019 || rtcm_type == 1044 || rtcm_type == 1042)
        {
            current_rtcm_no = 0;
			#if 0
            if (callback.callback_ptr_nav != NULL)
            {
                callback.callback_ptr_nav(data_buffer, rtcm_total_length);
            }
			#endif
            data_buffer[0] = '\0';
            format_buffer_data();
            return;
        }
        else if (rtcm_type == 1124 || rtcm_type == 1125)
        {
        }
        else
        { //舍弃其他类型的电文，暂时用不到
            current_rtcm_no = 0;
            data_buffer[0] = '\0';
            format_buffer_data();
            return;
        }
        for (i = 0; i < rtcm_total_length; i++)
        {
            rtcm_datas[current_rtcm_no][i] = *(data_buffer + i); //复制rtcm数据到rtcm数组
        }
        // print_hex(rtcm_datas[current_rtcm_no], rtcm_total_length);
        calculated_rtcm_total_lengths[current_rtcm_no] = rtcm_total_length;

        if (endOfRtcm(rtcm_datas[current_rtcm_no], rtcm_total_length))
        {
            current_rtcm_no++;
            calculated_rtcm_total_lengths[current_rtcm_no] = '\0';
            rtcm_datas[current_rtcm_no][0] = '\0';

			#if 0
            if (callback.callback_ptr_rtcm != NULL)
            {
                callback.callback_ptr_rtcm(rtcm_datas, calculated_rtcm_total_lengths);
            }
			#endif
        }
        else
        {
            current_rtcm_no++;
        }
    }

    data_buffer[0] = '\0';
    format_buffer_data();
    // printf("%d \n", data_buffer_rear_index);
}

/*
格式化Buffer数组，如果非有效头部开头，则将起前面的数据移除
*/
void format_buffer_data()
{
	char effectivie_data_buffer[BUFFER_LENGTH];
	int i;
    int head_index = get_effective_head_tag_index();
    
	printf("head_index %d \n", head_index);

    if (head_index == -1)
    { //没有找到有效头部，数组清零
        data_buffer_rear_index = 0;
        // printf("data_buffer_rear_index %d \n", data_buffer_rear_index);
        return;
    }
    if (head_index == 0)
    { //以有效头部开头，直接返回
        return;
    }
    
    for (i = head_index; i < data_buffer_rear_index; i++)
    {
        effectivie_data_buffer[i - head_index] = data_buffer[i];
    }

    data_buffer_rear_index -= head_index;
    for (i = 0; i < data_buffer_rear_index; i++)
    {
        data_buffer[i] = effectivie_data_buffer[i];
    }
}

/* 
获取有效的头部标签的位置，包括IMU头部，GGA头部，RTCM头部
哪个标签出现在前面，就返回那个
如果没有索引到，则返回-1，不区分类型
 */
int get_effective_head_tag_index()
{
    int index = -1;
    int i = 0;

    for (i = 0; i < data_buffer_rear_index; i++)
    {
        if (i < data_buffer_rear_index - 6 + 1)
        {
            if (startwith_imu_head(data_buffer + i))
            {
                printf("imu\n ");
                return i;
            }
            if (startwith_gga_head(data_buffer + i))
            {
                printf("gga\n ");
                return i;
            }
        }

        if (i < data_buffer_rear_index - 2 + 1)
        {
            if (startwith_rtcm_head(data_buffer + i))
            {
                printf("rtcm\n ");
                return i;
            }
        }
    }
    return index;
}

int startwith_gga_head(char *data)
{ //字节数组以$GNGGA开头
    int gga_index = strncmp(data, GGA_HEAD, 6);
    int is_gga = gga_index == 0;

    return is_gga;
}

int startwith_rtcm_head(char *data)
{ //字节数组以0XD3 0X00/0x01/0x02/0x03开头

    if (data[0] == 0xd3)
    {
        char head1 = (char)(0xfc & data[1]);
        if (head1 == 0x00)
        {
            return 1;
        }
    }

    return 0;
}

int startwith_imu_head(char *data)
{ //字节数组以$GPIMU开头
    int imu_index = strncmp(data, IMU_HEAD, 6);
    int is_imu = imu_index == 0;

    return is_imu;
}

/* 
检索首个回车换行出现的位置，用来截取gga。如果没有检索到，则返回-1
 */
int indexof_first_0d0a(char *data, int data_length)
{
    int i = 0;

    for (i = 0; i < data_length - 1; i++)
    {
        if ((0xff & *(data + i)) == 0x0d)
        {
            if ((0xff & *(data + i + 1)) == 0x0a)
            {
                return i;
            }
        }
    }
    return -1;
}

int endOfRtcm(char *rtcmData, int rtcmData_length)
{
    int ended = 1;
    if (rtcmData_length < 10)
    {
        return ended;
    }
    char end_tag_byte = rtcmData[9];
    ended = ((0x02 & end_tag_byte) >> 1);

    return ended;
}

int getRtcmType(const char *d300_addr)
{
    int byte4 = 0;
    int byte5 = 0;

    memcpy(&byte4, d300_addr + 3, 1);
    memcpy(&byte5, d300_addr + 4, 1);

    byte5 = byte5 >> 4;

    int type = byte4 * 16 + byte5;
    return type;
}

/* 
rtcm的完整长度
包括rtcm头D3 00 xx（长度）三个字节
+报文长度
+校验码三个字节
 */
int get_rtcm_total_length(int rtcm_length) //rtcm的完整长度
{
    return 3 + rtcm_length + 3;
}

void print_hex(char *data, int data_length)
{
    int i;

    for (i = 0; i < data_length; i++)
    {
        printf("%02X ", 0xff & *(data + i));
    }
    printf("\n");
}

int table[] = {
    0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
    0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272,
    0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646,
    0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3,
    0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
    0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, 0x681E59, 0xEE52A2,
    0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24,
    0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1,
    0xA11107, 0x275DFC, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
    0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0,
    0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4,
    0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8,
    0xC90F5E, 0x4F43A5, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
    0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, 0x92C69D, 0x148A66,
    0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB,
    0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E,
    0x4EBBF8, 0xC8F703, 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
    0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, 0xFAD8C4, 0x7C943F,
    0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B,
    0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C,
    0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
    0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0x4E2BC6, 0xC8673D,
    0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69,
    0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC,
    0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
    0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
};

char checksum[3];

char *get_checksum(char *data, int data_length)
{
    int crc = 0;
	int i = 0;

    for (i = 0; i < data_length - 3; i++)
    {
        crc = ((crc << 8) & 0xFFFFFF) ^ table[(crc >> 16) ^ (data[i] & 0xff)];
    }

    checksum[0] = ((crc & 0xff0000) >> 16);
    checksum[1] = ((crc & 0xff00) >> 8);
    checksum[2] = (crc & 0xff);

    return checksum;
}


int main()
{
	char * rtcm = "$GNGGA,085449.00,2307.33045,N,11320.72853,E,1,12,3.13,27.8,M,,M,,*6A\r\n";
	//char rtcm[] = {};

	printf("rtcm len = %ld\n", strlen(rtcm));

	extract_data(rtcm, strlen(rtcm));
}
